#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <time.h>
#include "robot.h"

#define BAUD_RATE B57600

static int serial;

int robot_init(char *port) {
	struct termios config;
	unsigned char full_mode[] = {128, 132};

	serial = open(port, O_RDWR | O_NOCTTY /*| O_NDELAY*/);
	if(serial < 0) {
		return -1;
	}
	config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
	//config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
	config.c_oflag = 0;
	config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
	config.c_cflag &= ~(CSIZE | PARENB);
	config.c_cflag |= CS8;
	config.c_cc[VMIN]  = 1;
	config.c_cc[VTIME] = 0;
	if (cfsetispeed(&config, BAUD_RATE) < 0 || cfsetospeed(&config, BAUD_RATE) < 0) {
		return -2;
	}
	if(tcsetattr(serial, TCSAFLUSH, &config) < 0) {
		return -3;
	}

	write(serial, full_mode, 2);

}

void robot_forward() {
	unsigned char forward[] = {145, 0, 100, 0, 100};
	write(serial, forward, 5);
}

void robot_backward() {
	unsigned char backward[] = {145, 255, 156, 255, 156};
	write(serial, backward, 5);
}

void robot_left() {
	unsigned char left[] = {145, 0, 100, 255, 156};
	write(serial, left, 5);
}

void robot_right() {
	unsigned char right[] = {145, 255, 156, 0, 100};
	write(serial, right, 5);
}

void robot_stop() {
	unsigned char stop[] = {145, 0, 0, 0, 0};
	write(serial, stop, 5);
}

void robot_bumpers(unsigned char *data, time_t *timestamp) {
	unsigned char bump[] = {142, 7};
	write(serial, bump, 2);
	//usleep(100000);
	read(serial, data, 1);
	(*data) &= 3;
	*timestamp = time(NULL);
}

int robot_close() {
	return close(serial);
}
